#include "rtps/storages/MemoryPool.h"

static void *
_mempool_alloc(struct rtps_MemoryPool *pmp)
{
    uint8_t *pblk = NULL;

    if (!pmp || !pmp->blk_list || (pmp->blk_free == 0))
        return NULL;

    pmp->blk_free--;
    pblk = pmp->blk_list;
    pmp->blk_list = *(uint8_t **)pblk;
    *(uint8_t **)pblk = (uint8_t *)pmp;

    return (void *)(pblk + sizeof(uint8_t *));
}

static void
_mempool_free(void *paddr)
{
    struct rtps_MemoryPool *pmp = NULL;
    uint8_t **ppblk = NULL;

    if (paddr == NULL)
        return;

    ppblk = (uint8_t **)((uint8_t *)paddr - sizeof(uint8_t *));
    pmp = (struct rtps_MemoryPool *)(*ppblk);
    pmp->blk_free++;
    *ppblk = pmp->blk_list;
    pmp->blk_list = (uint8_t *)ppblk;
    memset(paddr, 0, pmp->blk_size);
}

struct rtps_MemoryPool *
rtps_MemoryPool_init(uint32_t blk_size, uint32_t blk_cnt)
{
    struct rtps_MemoryPool *pmp = NULL;
    uint8_t  *pblk = NULL;
    uint32_t offset = 0;

    /* allocate memory for memory pool controller */
    pmp = (struct rtps_MemoryPool *)calloc(1, sizeof(struct rtps_MemoryPool));
    if (pmp == NULL)
        return NULL;

    /* allocate memory for memory pool blocks */
    pmp->mrp_size = blk_cnt * (sizeof(uint8_t *) +  blk_size);
    pmp->mrp_addr = malloc(pmp->mrp_size);
    if (pmp->mrp_addr == NULL) {
        free(pmp);
        return NULL;
    }

    memset(pmp->mrp_addr, 0, sizeof(pmp->mrp_size));
    pmp->blk_total = blk_cnt;
    pmp->blk_free = pmp->blk_total;
    pmp->blk_size = blk_size;

    /* initialize free block list */
    pblk = (uint8_t *)pmp->mrp_addr;
    for (offset = 0; offset < pmp->blk_total - 1; offset++) {
        *(uint8_t **)(pblk + offset * (blk_size + sizeof(uint8_t *)))
            = pblk + (offset + 1) * (blk_size + sizeof(uint8_t *));
    }
    *(uint8_t **)(pblk + offset * (blk_size + sizeof(uint8_t *))) = NULL;
    pmp->blk_list = pblk;

    return pmp;
}

void
rtps_MemoryPool_deinit(struct rtps_MemoryPool *pmp)
{
    if (pmp == NULL)
        return;

    if (pmp->mrp_addr)
        free(pmp->mrp_addr);

    free(pmp);
}

uint32_t
rtps_MemoryPool_getSize(struct rtps_MemoryPool *pmp)
{
    return pmp->blk_total;
}

bool
rtps_MemoryPool_isFull(struct rtps_MemoryPool *pmp)
{
    if (pmp->blk_free == 0)
        return true;

    return false;
}

bool
rtps_MemoryPool_isEmpty(struct rtps_MemoryPool *pmp)
{
    if (pmp->blk_free == pmp->blk_total)
        return true;

    return false;
}

uint32_t
rtps_MemoryPool_getNumElements(struct rtps_MemoryPool *pmp)
{
    if(!pmp)
        return 0;
        
    if (pmp->blk_free == pmp->blk_total)
        return 0;

    return false;
}

bool
rtps_MemoryPool_add(struct rtps_MemoryPool *pmp, void *data)
{
    void *pblk = _mempool_alloc(pmp);

    if (pblk == NULL)
        return false;

    memcpy(pblk, data, pmp->blk_size); 
    return true;
}

bool
rtps_MemoryPool_remove(
    struct rtps_MemoryPool *pmp,
    rtps_MemoryPool_jumppad jumppad,
    void *isCorrectElement)
{
    uint8_t *ptmp, *pblk, *pdata = NULL;
    uint32_t offset;

    RTPS_MEMORYPOOL_ITER(pmp, offset, ptmp, pblk, pdata){
        if (jumppad(isCorrectElement, pdata)) {
            _mempool_free(pdata);
            return true;
        }
    }

    return false;
}

bool
rtps_MemoryPool_find(
    struct rtps_MemoryPool *pmp,
    rtps_MemoryPool_jumppad jumppad,
    void *isCorrectElement)
{
    uint8_t *ptmp, *pblk, *pdata = NULL;
    uint32_t offset;

    RTPS_MEMORYPOOL_ITER(pmp, offset, ptmp, pblk, pdata){
        if (jumppad(isCorrectElement, pdata))
            return true;
    }

    return false;
}

bool
rtps_MemoryPool_getElement(
    struct rtps_MemoryPool *pmp,
    rtps_MemoryPool_jumppad jumppad,
    void *isCorrectElement,
    void *findElement,
    uint32_t size)
{
    uint8_t *ptmp,*pblk,*pdata;
    uint32_t offset;

    RTPS_MEMORYPOOL_ITER(pmp, offset, ptmp, pblk, pdata){
        if (jumppad(isCorrectElement, pdata)){
            memcpy(findElement, pdata, size);
            return true;
        }
    }

    return false;
}

/* can modify member, but need to use in lock*/
bool
rtps_MemoryPool_getElement_not_safe(
    struct rtps_MemoryPool *pmp,
    rtps_MemoryPool_jumppad jumppad,
    void *isCorrectElement,
    void **findElement)
{
    uint8_t *ptmp,*pblk,*pdata;
    uint32_t offset;

    RTPS_MEMORYPOOL_ITER(pmp, offset, ptmp, pblk, pdata){
        if (jumppad(isCorrectElement, pdata)){
            *findElement = pdata;
            return true;
        }
    }

    return false;
}

void
rtps_MemoryPool_test(struct rtps_MemoryPool *pmp)
{
    uint8_t *ptmp;
    uint8_t *pblk;
    uint8_t *pdata;
    uint32_t offset;

    RTPS_MEMORYPOOL_ITER(pmp, offset, ptmp, pblk, pdata) {
        pdata++;
    };
}

